﻿using System;
using System.Collections.Generic;
using System.Text;

namespace RobotsSample.RobotSamples
{
    /// <summary>
    ///  四轮移动小车（麦轮)
    /// </summary>
    public class Sample_McrumWheelCarRobot : RobotSampleAbstract
    {
        public override void Run()
        {
            base.Run();
            /*
           *######################################################################
           *####################### 机器人模块ID配置  ###########################
           *######################################################################
           */

            //输入小车ID次序
            Console.WriteLine("The id sequence above may be incorrect");
            Console.WriteLine("Please enter the Mcrum Wheel Car Robot ids in sequence（L1,L4,R1,R4）.");
          

            //记录小车机器人ID序列：F,B,l1,l2,l3,l4,r1,r2,r3,r4
            var ids = new byte[10];
            Console.WriteLine("Please enter the Mcrum Wheel id at L1.");
          
            var idStr = Console.ReadLine();
            byte.TryParse(idStr,out ids[2]);

            Console.WriteLine("Please enter the Mcrum Wheel id at L4.");
             idStr = Console.ReadLine();
            byte.TryParse(idStr, out ids[5]);

            Console.WriteLine("Please enter the Mcrum Wheel id at R1.");
            idStr = Console.ReadLine();
            byte.TryParse(idStr, out ids[6]);

            Console.WriteLine("Please enter the Mcrum Wheel id at R4.");
            idStr = Console.ReadLine();
            byte.TryParse(idStr, out ids[9]);
                      
            Console.WriteLine("Ids  completed");


            /*
             *######################################################################
             *####################### 机器人算法绑定  ###########################
             *######################################################################
             */
            Console.WriteLine("Bind to creat a car robot...");
            RobotId = MiniBotAlgorithmLib.AddRobot((int)RobotTypeEnum.Car, ids, 5);   //5为四卖轮

            var rs = MiniBotAlgorithmLib.BindRobotModuleApis(RobotId, SetPositionCallback, SetPositionWithSpeedCallback, SetMultiPositionCallback,
                    SetMultiPositionWithSpeedCallback, SetSpeedCallback, SetRunningSpeedCallback, SetTorqueEnableCallback, SetModeCallback);
          
            /*
	         *######################################################################
	         *####################### 机器人操作控制开始  ###########################
	         *######################################################################
	         */


            //将关节模式转换成速度模式
            for (var i = 0; i < ModulesIds.Count; i++)
            {
                MiniBotControlLib.SetMotorMode((byte)ModulesIds[i], 1);
            }

            while (true)
            {
                //操作命令
                var cmd = 1;

                Console.WriteLine($"Commands：");
                Console.WriteLine("1.Go Forward");
                Console.WriteLine("2.Go Back");
                Console.WriteLine("3.Stop");
                Console.WriteLine("4.Turn Left");
                Console.WriteLine("5.Turn Right");
                Console.WriteLine("6.Go Left");
                Console.WriteLine("7.Go Right");
                Console.WriteLine("8.Rotate Angles");

                var cmdStr = Console.ReadLine();
                int.TryParse(cmdStr, out cmd);

                switch (cmd)
                {
                    case 8:
                        {
                            Console.WriteLine($"Commands：{Environment.NewLine}Enter the angle:");
                            int angle = 0;
                            var mIdStr = Console.ReadLine();
                            int.TryParse(mIdStr, out angle);
                            SendCmd(cmd, angle);
                        }
                        break;
                    default:
                        {
                            SendCmd(cmd, 0);
                        }
                        break;
                }

                Console.WriteLine("Enter any key to continue ....");

            }

        }

    }
}
